
#include <ServoPWMF.h>

ServoPWMF s;
int capacity = 400;
int data0[400];
int data1[400];
int ledPin = 13;                 // LED connected to digital pin 13
int samplingperiod = 3000;         //sample rate = 1 / (samplingrate/1000) Khz, total sampling time = samplingperiod * capcity us
int commandInterval = 61;      //one second per 61 units
float avgThres = 1;            //the voltage treshold for comparing avg
int samplingThres = 400;        // samplingThres/1023 * 5 = voltage threshold
boolean islastsamplesignal = false;
float avg0;
float avg1;

unsigned long startTime;


void setup() 
{ 
  Serial.begin(9600);
  s.setupTimer();
  // nothing for setup a
  pinMode(ledPin, OUTPUT); 
  relaxPos();
  delay(61*3);

} 

void loop() 
{
  sampling();      // take capacity number of samples 


  if( islastsamplesignal )
  {
    avg0= average(data0);
    avg1= average(data1);
    int result = interpret (avg1, avg0);

    Serial.print("avg1: ");
    Serial.print(avg1/1023*5);
    Serial.print("       avg0: ");
    Serial.println(avg0/1023*5);
    Serial.print("result: ");
    Serial.println(result, HEX);

    switch(result)
    {
    case 0x00:
      relaxPos(); 
      break;
    case 0x01:
      grabPos(); 
      break;
    case 0x10:
      pinchPos();  
      break;
    case 0x11:
      pointPos(); 
    }
    s.printAngles();
    delay(commandInterval); //stop before we take another signal

  }
}
//end of loop()

void relaxPos()
{
  Serial.print("Relax: ");
  s.writeT(90);
  s.writeF1(90);
  s.writeF2(90);
  s.writeF3(90);
  s.writeF4(90);
}
void grabPos()
{
  Serial.print("Grab: ");
  float percent = (avg0/1023*5-avgThres) / (5-avgThres);
  float cap = 0.6 + 0.4*percent;
  // Serial.print("cap: ");
  // Serial.println(180-180*cap);
  // Serial.print("F1: ");
  // Serial.println(180-180*cap);
  s.writeT(180*cap);
  s.writeF1(180-180*cap);
  s.writeF2(180*cap);
  s.writeF3(180*cap);
  s.writeF4(180*cap);

}
void pinchPos()
{
  Serial.print("Pinch: ");
  float percent = (avg1/1023*5-avgThres) / (5-avgThres);
  float cap = 0.6 + 0.4*percent;
  s.writeT(135);
  s.writeF1(180-180*cap);
  s.writeF2(180);
  s.writeF3(180);
  s.writeF4(180);
}

void pointPos()
{
  Serial.println("Point: ");
  s.writeT(180);
  s.writeF1(90);
  s.writeF2(180);
  s.writeF3(180);
  s.writeF4(180);
}


int interpret(float Avg1, float Avg0)
{
  if (!overThres(Avg1) && !overThres(Avg0))
    return 0x00;
  if (!overThres(Avg1) && overThres(Avg0))
    return 0x01;
  if (overThres(Avg1) && !overThres(Avg0))
    return 0x10;
  if (overThres(Avg1) && overThres(Avg0))
    return 0x11;    
}

boolean overThres (float Avg)
{
  //true if exceed threshold
  return Avg > avgThres/5.0*1023;
}

float average(int data[])
{
  unsigned long sum =0;
  for( int i=0; i<capacity; i++)
  {
    sum = sum + data[i];
  }
  return (float)sum / capacity;
}

void sampling()
{
  int val0 = analogRead(0);
  int val1 = analogRead(1);

  //detect new signal
  if( val0 > samplingThres || val1 > samplingThres)
  { 
    digitalWrite(ledPin, HIGH);   // sets the LED on
    for( int i=0; i< capacity; i++)
    {
      data0[i] = analogRead(0);
      data1[i] = analogRead(1);
      // timerStart();
      delayMicroseconds(samplingperiod); //sampling rate = 1/ sampling period
      // timerStop();
    }
    digitalWrite(ledPin, LOW);   // sets the LED off
    //printdata();
    islastsamplesignal=true;
  }

  else
  {
    for( int i=0; i< capacity; i++)
    {
      data0[i] = 0;
      data1[i] = 0;
    }
    islastsamplesignal = false;
  }

}


void printdata()
{
  Serial.print("S");
  for(int i=0; i< capacity; i++)
  {
    Serial.println(data0[i]); 
  }
  Serial.print("E");
} 

void timerStart()
{
  startTime=micros();
}

void timerStop()
{
  Serial.println(micros() - startTime);
}


void delayus(unsigned long delay)
{
  unsigned long start = micros();
  while( (micros()-start) < delay)
  {
  }
}


